/*
 * CBadPhysics.cpp
 *
 *  Created on: 07.01.2012
 *      Author: Kirill Blinov
 *      Copyright: LGPL
 */

#include "CBadPhysics.h"
#include <ctime>

CBadPhysics::CBadPhysics() : lastTime(0)
{
	collisionCfg = new btDefaultCollisionConfiguration();
	axisSweep = new btAxisSweep3( btVector3(-100,-100,-100), btVector3(100,100,100), 256 );
	dynamicsWorld = new btDiscreteDynamicsWorld( new btCollisionDispatcher(collisionCfg),
	                                             axisSweep, new btSequentialImpulseConstraintSolver(), collisionCfg);
	dynamicsWorld->setGravity(btVector3( 0.0f, 0.0f, -9.8f ));

#ifdef DEBUG_PHYSICS
	GLDebugDrawer *DebugDrawer = new GLDebugDrawer();

	dynamicsWorld->setDebugDrawer( DebugDrawer );
#endif

}

CBadPhysics::~CBadPhysics()
{
	delete dynamicsWorld;
	delete collisionCfg;
	delete axisSweep;
}

UINT
CBadPhysics::AddRigidObject( const float Mass, glm::quat  Orientation, glm::vec3 Position,
		                     const glm::vec3 Dimension, const glm::vec3 Inertia )
{
	btCollisionShape     *shape;
	btMotionState        *motionState;
	glm::vec3 Sizes =     Dimension;
	shape = new btBoxShape( btVector3( abs(Sizes.x), abs(Sizes.y), abs(Sizes.z) ) );

	btTransform trans;
	trans.setIdentity();

	btQuaternion qtn;
	glm::vec3 EulerAngles = glm::eularAngles( Orientation );
	qtn.setEuler( EulerAngles.x, EulerAngles.y , EulerAngles.z );

	trans.setRotation( qtn );
	trans.setOrigin( btVector3( Position.x, Position.y, Position.z ) );
	motionState = new btDefaultMotionState( trans );

	btVector3 TmpInertia( Inertia.x, Inertia.y, Inertia.z );
	if ( ( 0.0f == Inertia.x ) &&   ( 0.0f == Inertia.y ) &&  ( 0.0f == Inertia.z ) )
	{
		shape->calculateLocalInertia( Mass, TmpInertia);
	}

	btRigidBody::btRigidBodyConstructionInfo RigidBodyConstructionInfo(  btScalar(Mass), motionState, shape, TmpInertia );
	RigidBodyConstructionInfo.m_restitution = 0.01f;//1.3
	RigidBodyConstructionInfo.m_friction = 0.8f;
	RigidBodyConstructionInfo.m_linearDamping = 0.2;
	RigidBodyConstructionInfo.m_angularDamping = 0.1f;

	btRigidBody *NewBody = new btRigidBody( RigidBodyConstructionInfo );
	//NewBody->setActivationState( DISABLE_DEACTIVATION );
	dynamicsWorld->addRigidBody( NewBody );

	Objects.push_back( NewBody );
	return ( Objects.size() - 1 );
}

glm::mat4
CBadPhysics::GetObjectMatrix( const UINT ObjectID )
{
	btTransform Transformation;
	glm::mat4   Matrix;

	btRigidBody *Body = Objects[ObjectID];
	Body->getMotionState()->getWorldTransform( Transformation );
	Transformation.getOpenGLMatrix( (btScalar*)&Matrix );

	return Matrix;
}

glm::quat
CBadPhysics::GetOrientation( const UINT ObjectID )
{
	btRigidBody *Body = Objects[ObjectID];
	btVector3 Orientation = Body->getOrientation().getAxis();
	glm::quat Result( glm::vec3( Orientation.x(), Orientation.y(), Orientation.z() ) );
	return Result;
}

void
CBadPhysics::Simulate()
{
	clock_t CurentTime = clock();
	if ( lastTime != 0 )
	{
		dynamicsWorld->stepSimulation( CurentTime - lastTime, 3 );
	}
	lastTime = CurentTime;
}

std::vector<glm::vec3>
CBadPhysics::GetContactPoints()
{
	std::vector<glm::vec3> Contacts;
	return Contacts;
}

void
CBadPhysics::MoveRotate( const UINT ObjectID, const glm::vec3 Translation, const glm::quat *Rotation )
{
	btQuaternion qtn;

	if ( Rotation != NULL )
	{
		glm::vec3 EulerAngles = glm::eularAngles( *Rotation );
		qtn.setEuler( EulerAngles.x, EulerAngles.y , EulerAngles.z );
	}

	btTransform Transformation( qtn, btVector3( Translation.x, Translation.y, Translation.z ) );
	Objects[ObjectID]->setCenterOfMassTransform( Transformation );
}

void
CBadPhysics::ApplyForce( const UINT ObjectID, const glm::vec3 Force )
{
	Objects[ObjectID]->activate( true );
	Objects[ObjectID]->applyCentralForce( btVector3( Force.x, Force.y, Force.z ) );
}

void
CBadPhysics::ApplyForce( const UINT ObjectID, const glm::vec3 Force, const glm::vec3 Point )
{
	Objects[ObjectID]->activate( true );
	Objects[ObjectID]->applyForce( btVector3( Force.x, Force.y, Force.z ), btVector3( Point.x, Point.y, Point.z ) );
}

void
CBadPhysics::ApplyImpulse( const UINT ObjectID, const glm::vec3 Impulse )
{
	Objects[ObjectID]->activate( true );
	Objects[ObjectID]->applyCentralImpulse( btVector3( Impulse.x, Impulse.y, Impulse.z ) );
}

void
CBadPhysics::ApplyImpulse( const UINT ObjectID, const glm::vec3 Impulse, const glm::vec3 Point )
{
	Objects[ObjectID]->activate( true );
	Objects[ObjectID]->applyImpulse( btVector3( Impulse.x, Impulse.y, Impulse.z ), btVector3( Point.x, Point.y, Point.z ) );
}

void
CBadPhysics::ApplyVelocity( const UINT ObjectID, const glm::vec3 Velocity )
{
	btVector3 CurrentVelocity;
	CurrentVelocity = Objects[ObjectID]->getLinearVelocity();
	CurrentVelocity += btVector3( Velocity.x, Velocity.y, Velocity.z );

	Objects[ObjectID]->activate( true );
	Objects[ObjectID]->setLinearVelocity( CurrentVelocity );
}

void
CBadPhysics::ApplyTorque( const UINT ObjectID, const glm::vec3 Torque )
{

}

void
CBadPhysics::AddPlane( const glm::vec3 Normal, const float PlaneConstant )
{
	btCollisionShape     *Plane;
	btMotionState        *motionState;
	Plane = new btStaticPlaneShape( btVector3( Normal.x, Normal.y, Normal.z ), PlaneConstant );

	btTransform trans;
	trans.setIdentity();
	motionState = new btDefaultMotionState( trans );

	btRigidBody::btRigidBodyConstructionInfo RigidBodyConstructionInfo(  btScalar(0), motionState, Plane, btVector3( 0, 0, 0 ) );
	RigidBodyConstructionInfo.m_restitution = 0.4f;
	RigidBodyConstructionInfo.m_friction = 0.1f;

	btRigidBody *NewBody = new btRigidBody( RigidBodyConstructionInfo );
	dynamicsWorld->addRigidBody( NewBody );

	Objects.push_back( NewBody );
}
